#include "svpwm.h"
#include "math.h"
#include "fast_math.h"
#include  "tim.h"
#include "filter.h"
#include "motor_task.h"

svpwm_t svpwm;
motor_infor_t MFOR;
cur_voltage_data cur_voltage_f;

uint16_t adc_iabc_uabc[6]={0};
uint16_t* adc_iabc[3]={NULL};
uint16_t* adc_uabc[3]={NULL};
uint32_t iabc_offset[3] ={0} ;


float adc_iabc_f[3],adc_uabc_f[3]={0};
    int sector = 0;


void svpwm_func(float ud ,float uq ,float theta,float Udc,float Tpwm, svpwm_t* sv,motor_uab_t* uab){
	sector = 0;
	
//	float us = ud*ud+uq*uq;
//	float um = Udc*Udc;
//	float abs_ud = fabs(ud);
//	float beta = atanf(uq/abs_ud);
//	if(us>um){
//		ud=-sqrt(um)*cos(beta);
//    uq=sqrt(um)*sin(beta);
//		if(ud>0){
//			ud=sqrt(um)*cos(beta);
//		}
//	}
	
    float Valpha = - uq*sinf(theta) + ud*cosf(theta) ;
    float Vbeta = ud*sinf(theta) + uq*cosf(theta);//is already done in the front invpark

    uab->_ual = Valpha;
    uab->_ube = Vbeta;

    float ta,tb,tc = 0;
    float Tcmp1,Tcmp2,Tcmp3=0;
//codegen

//========Parameters statement================
    float Vref1 = Vbeta;							                   
    float Vref2 = (sqrt(3)*Valpha - Vbeta)/2; 				    
    float Vref3 = (-sqrt(3)*Valpha - Vbeta)/2;
//========Sector calculation=================
    if (Vref1 > 0){
        sector = 1;
    }
    if(Vref2 > 0){
        sector = sector+2;
    }
    if(Vref3>0){
         sector = sector+4;
    }
//======== X Y Z calculation===================
    float X = sqrt(3)*Vbeta*Tpwm/Udc;
    float Y = Tpwm/Udc*(3/2*Valpha+sqrt(3)/2*Vbeta);
    float Z = Tpwm/Udc*(-3/2*Valpha+sqrt(3)/2*Vbeta);	
    float T,T1,T2 = 0;
//==========Duty ratio calculation================
    switch (sector){
        case 1:
            T1 = Z; T2 = Y; 
            break;
        case 2:
            T1 = Y; T2 = -X; 
            break;
        case 3:
            T1 = -Z; T2 = X; 
            break;
        case 4:
            T1 = -X; T2 = Z;
            break;
        case 5:
            T1 = X; T2 = -Y;
            break;
        default :
            T1 = -Y; T2 = -Z;
            break;
    }

    T = T1+T2;
    if (T > Tpwm){
        T1 = T1/T*Tpwm;
        T2 = T2/T*Tpwm;
				T = T1+T2;
		}

    ta = (Tpwm-(T1+T2))/4.0f;
    tb = ta+T1/2.F;
    tc = tb+T2/2.F;
		
		ta=round(ta);
		tb=round(tb);
		tc=round(tc);
//==========Duty ratio calculation================
switch (sector){
    case 1 :
        Tcmp1=tb;
        Tcmp2=ta;
        Tcmp3=tc; 
        break;
    case 2 :
        Tcmp1=ta;
        Tcmp2=tc;
        Tcmp3=tb; 
        break;
    case 3 :
        Tcmp1=ta;
        Tcmp2=tb;
        Tcmp3=tc; 
        break;
    case 4 :
        Tcmp1=tc;
        Tcmp2=tb;
        Tcmp3=ta;
        break;  
    case 5 :
        Tcmp1=tc;
        Tcmp2=ta;
        Tcmp3=tb;
        break;   
   case 6:
       Tcmp1=tb;
       Tcmp2=tc;
       Tcmp3=ta;
       break;
    }
    sv->TEMP1_p = Tcmp1;
    sv->TEMP2_P = Tcmp2;
    sv->TEMP3_P = Tcmp3;
}

void FOC_INIT(TIM_HandleTypeDef* adc_tim,ADC_HandleTypeDef* hadc1){

    HAL_TIM_Base_Start(adc_tim);
    HAL_ADC_Start_DMA(hadc1,(uint32_t*)&adc_iabc_uabc,6);
		
		for(uint16_t i = 0 ; i < 3 ; i ++){
			adc_iabc[i] = &adc_iabc_uabc[i];
			adc_uabc[i] = &adc_iabc_uabc[i+3];
		}

    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
    HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_1);
		HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
    HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
    HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_3);

    HAL_Delay(100);
    for(uint32_t i = 0 ; i < 64 ; i ++){
			    HAL_Delay(10);
        iabc_offset[0] += *adc_iabc[0];
        iabc_offset[1] += *adc_iabc[1];
        iabc_offset[2] += *adc_iabc[2];
        }
		iabc_offset[0]/=64.f;
		iabc_offset[1]/=64.f;
		iabc_offset[2]/=64.f;
}
float init_tare = 20.f;
void FOC_GO_currunt_voltage(cur_voltage_data* cur_voltage){
	ADC_One_Filter(*adc_iabc[0],&adc1_filter_ia);
	ADC_One_Filter(*adc_iabc[1],&adc1_filter_ib);
	ADC_One_Filter(*adc_iabc[2],&adc1_filter_ic);
	
//	adc1_filter_ia.out = *adc_iabc[0];
//	adc1_filter_ib.out = *adc_iabc[1];
//	adc1_filter_ic.out = *adc_iabc[2];
//	adc1_filter_ia.out = iir_filter_3((float)(*adc_iabc[0]),1);
//	adc1_filter_ib.out = iir_filter_3((float)(*adc_iabc[1]),2);
//	adc1_filter_ic.out = iir_filter_3((float)(*adc_iabc[2]),3);
	ADC_One_Filter(*adc_uabc[0],&adc1_filter_ua);
	ADC_One_Filter(*adc_uabc[1],&adc1_filter_ub);
	ADC_One_Filter(*adc_uabc[2],&adc1_filter_uc);
																																																								
  cur_voltage->iabc_finnal[0] = (1.650f-0.0f - adc1_filter_ia.out/4096.000f*3.300f)/(init_tare*0.005f)-(iabc_offset[0]-2047.f)*3.3f/4095.f/ REGISTER/init_tare;
	cur_voltage->iabc_finnal[1] = (1.650f-0.0f - adc1_filter_ib.out/4096.000f*3.300f)/(init_tare*0.005f)-(iabc_offset[1]-2047.f)*3.3f/4095.f/ REGISTER/init_tare;
	cur_voltage->iabc_finnal[2] = (1.650f-0.0f - adc1_filter_ic.out/4096.000f*3.300f)/(init_tare*0.005f)-(iabc_offset[2]-2047.f)*3.3f/4095.f/ REGISTER/init_tare;
	cur_voltage->uabc_finnal[0] = (adc1_filter_ua.out )*3.3f/4096.f*11.f;                                         
	cur_voltage->uabc_finnal[1] = (adc1_filter_ub.out )*3.3f/4096.f*11.f;
	cur_voltage->uabc_finnal[2] = (adc1_filter_uc.out )*3.3f/4096.f*11.f;
}

void calrk_func(motor_iabc_t* iabc,motor_iab_t* ialbe){
   float ia,ib,ic=0;
   float ialpha,ibeta = 0;
   ia = cur_voltage_f.iabc_finnal[0];
   ib = cur_voltage_f.iabc_finnal[1];
   ic = cur_voltage_f.iabc_finnal[2];

    iabc->_ia = ia;
    iabc->_ib = ib;
    iabc->_ic = ic;//only want to copy something
    

   ialpha = ia*2.f/3.f-(ib+ic)/3.f;
   ibeta = (ib-ic)*sqrt(3.f)/3.f;

   ialbe->_ial = ialpha;
   ialbe->_ibe = ibeta;
}

void park_func(motor_iab_t* ialbe,float theta,motor_idq_t* idq){
   float ialpha,ibeta;
   float id,iq=0;
   ialpha = ialbe->_ial;
   ibeta = ialbe->_ibe;

   id = ialpha *cos(theta)+ibeta*sin(theta);
   iq = -ialpha * sin(theta)+ibeta *cos(theta);

   idq->_id = id;
   idq->_iq = iq;
}

// void invpark(float theta ,foc_infor_t* foc_cal){
//    float ualpha,ubeta = 0;
//    float ud,uq;
//    ud = foc_cal->voltage_ud_uq._ud;
//    uq = foc_cal->voltage_ud_uq._uq;

//    ualpha = cos(theta)*ud-sin(theta)*uq;
//    ubeta = sin(theta)*ud+cos(theta)*uq;

//    foc_cal->voltage_alpha_beta._ualpha = ualpha;
//    foc_cal->voltage_alpha_beta._ubeta = ubeta;
// }

void set_pwm_tim_value(TIM_HandleTypeDef *TIMx, uint32_t CompareValue_0,
                                                uint32_t CompareValue_1,
                                                uint32_t CompareValue_2){
    __HAL_TIM_SET_COMPARE(TIMx,TIM_CHANNEL_1,CompareValue_0);
    __HAL_TIM_SET_COMPARE(TIMx,TIM_CHANNEL_2,CompareValue_1);
    __HAL_TIM_SET_COMPARE(TIMx,TIM_CHANNEL_3,CompareValue_2);
}
